#include "EZ_Thread.h"

uint32_t 		cur_sp;
uint32_t 		next_sp;
uint32_t 		switch_interrupt_flag;
ez_thread_t *cur_task;	

uint32_t 	ez_scheduler_lock_nest;

list_t 		thread_suspend_list;
list_t 		thread_priority_table[THREAD_PRIORITY_MAX];
uint32_t 	thread_ready_priority_group;

void tri_pandsv() 
{
    MEM32(NVIC_INT_CTRL) = NVIC_PENDSVSET;  
}

void switch_task(uint32_t next_task)
{
	switch_interrupt_flag = 1;
	next_sp = next_task;
	
	tri_pandsv();
}

void scheduler_insert_thread(ez_thread_t *task)
{
	
	list_remove(&task->tlist);
	
	list_insert_before(&thread_priority_table[task->init_priority],&task->tlist);
	
	bit_map_set(&thread_ready_priority_group,task->init_priority);
	
}

void scheduler_remove_thread(ez_thread_t *task)
{
	
	list_remove(&task->tlist);
	
	if(list_isempty(&thread_priority_table[task->init_priority]))
	{
		bit_map_clear(&thread_ready_priority_group,task->init_priority);
	}
	
}


__asm void PendSV_Handler ()
{   
    IMPORT	cur_sp
    IMPORT	next_sp
    IMPORT	switch_interrupt_flag

		LDR 		R0,	=cur_sp
		LDR 		R0,	[R0]
    CBZ     R0, PendSVHandler_schedule  
		
PendSVHandler_save

    MRS     R0, PSP        
    STMDB   R0!, {R4-R11} 
		LDR 		R1,	 =cur_sp	
		LDR 		R1,	 [R1]
		STR 		R0,	 [R1]
               

PendSVHandler_schedule     
		
    LDR     R0, =cur_sp          
    LDR     R1, =next_sp             
    LDR     R1, [R1]  
    STR     R1, [R0]                  
 
		LDR			R0, [R0]
		LDR			R0, [R0]
    LDMIA   R0!, {R4-R11}             

    MSR     PSP, R0                   
    ORR     LR, LR, #0x04             
    BX      LR      
		NOP
}  

void ez_enter_critical(void)
{
	uint32_t state;
	state = ez_hw_interrupt_disable();
	
	ez_scheduler_lock_nest++;
	
	ez_hw_interrupt_enable(state);
}


void ez_exit_critical(void)
{
	uint32_t state;
	state = ez_hw_interrupt_disable();
	
	ez_scheduler_lock_nest--;
	
	if(ez_scheduler_lock_nest <= 0)
	{
		ez_scheduler_lock_nest = 0;
	}
	
	ez_hw_interrupt_enable(state);
}


int scheduler_look_check(void)
{
	return (ez_scheduler_lock_nest == 0);
}

void system_scheduler_init(void)
{
	uint32_t offset;
	
	for(offset = 0; offset <= THREAD_PRIORITY_MAX -1; offset++)
	{
		list_init(&thread_priority_table[offset]);
	}
	
	list_init(&thread_suspend_list);
	
	thread_ready_priority_group = 0;
	cur_sp = EZ_NULL;
	next_sp = EZ_NULL;
	
}

void system_scheduler_start(void)
{
	ez_thread_t *to_thread;
	uint32_t highest_ready_priority;
	
	highest_ready_priority = bit_map_find(thread_ready_priority_group);
	
	to_thread = get_list_entry(thread_priority_table[highest_ready_priority].next,ez_thread_t,tlist);
	
	cur_task = to_thread;
	
	switch_task((uint32_t)&(to_thread->sp));
}


void task_scheduler(void)
{
	uint32_t 		state;
	uint32_t 		highest_ready_priority;
	ez_thread_t *to_thread;
	
	state = ez_hw_interrupt_disable();

	if(scheduler_look_check())
	{
			highest_ready_priority = bit_map_find(thread_ready_priority_group);
			
			to_thread = get_list_entry(thread_priority_table[highest_ready_priority].next,ez_thread_t,tlist);
		
			if(cur_task != to_thread)
			{
				to_thread->stat = EZ_THREAD_RUNNING;
				cur_task = to_thread;
				switch_task((uint32_t)&(to_thread->sp));
			}
	}
	
	
	ez_hw_interrupt_enable(state);

}
